/*
 * balance.c
 *
 *  Created on: 2022年11月18日
 *      Author: paper
 */
#include "balance.h"
float image_Error = 0; //摄像头偏差
float Dynamic_zero = 0.0f;
float qianhou_zero = 0;
float Dynamic_zero_set = 0.0f;
float k=0;
float k2 = 0;//压弯系数


//左右平衡
//角速度环
float ROLL_ANGLESPEED_LOOP(float value) {

    float SetValue = value;
    float outt = SetValue-20*imu.gyro.gyro[X];
    float out = PidIncCtrl(&roll_anglespeed_wheel, outt);
    return out;
}

float ZHONGZHI_roll = 0;//左右机械中值
//角度环
float ROLL_ANGLE_LOOP(float value) {
    float SetValue = value;
    //动态零点// 计算实际值，实际值由欧拉角的横滚角、ZHONGZHI_roll 常量减去系数乘以动态零点得到
    float ActualValue = eulerAngle.roll+Dynamic_zero+1.5f  ;//

    float out = Positional_PID(&roll_angle_wheel, SetValue, ActualValue, 2);

    return out;
}

//速度环
float ROLL_SPEED_LOOP(float value) {

    float SetValue = value;
    int ActualValue = (encoder_bldc_b+encoder_bldc_f) / 2;
    float out = Positional_PID(&roll_speed_wheel, SetValue, (float)ActualValue, 40);//积分限制过小导致抗扰能力不足，若阻力较大时，积分项快速饱和，无法持续补偿阻力，导致速度下降。积分限幅过小无法消除稳态误差（如速度持续下降）
    return out;
}

//前后平衡
//角度环
float ZHONGZHI_PITCH = 2.0f;
float BDC_BANLANCE_LOOP(float Angle, float Gyro){
    float SetValue = Angle;//目标值
    float ActualValue = Gyro+ZHONGZHI_PITCH-qianhou_zero;///实际值
    float out = Positional_PID(&bdc_banlance_wheel, SetValue, ActualValue, 500);
    return out;
}
//速度环
float BDC_SPEED_LOOP(float Value , float actualValue) {
    float SetValue = Value;
    float ActualValue = actualValue;
    float out = Positional_PID(&bdc_speed_wheel, SetValue, ActualValue,130);//如果Max_I设置过小，积分作用不足，可能在负载变化时无法维持速度。积分限幅过小无法消除稳态误差（如速度持续下降）

    return out;
}

//角速度环
float BDC_LOOP(float Value , float actualValue ) {
    float outt =Value- actualValue;
    float out = PidIncCtrl(&bdc_wheel,outt);
    return out;
}




//转向环
//角速度
float BLDC_YAW_ANGLESPEED_LOOP(float value) {
    float SetValue = value;
    float ActualValue = 20*imu.gyro.gyro[Z];
    float out = Positional_PID(&bldc_yaw_anglespeed_wheel, SetValue, ActualValue, 100);
    return out;
}

//角度
float BLDC_YAW_ANGLE_LOOP(float value) {
    float SetValue = value;
    float ActualValue = image_error1;//车辆的yaw轴离目标yaw轴的偏差值，通过pid的单位换算我们可以基本将摄像头的偏差值视为yaw轴的偏差值
    float out = Positional_PID(&bldc_yaw_angle_wheel, SetValue, ActualValue, 40);//adc_error_2  -1到1；
    return out;
}


//速度
float BLDC_YAW_SPEED_LOOP(float value) {
    float SetValue = value;
    int ActualValue = encoder_bldc_f - encoder_bldc_b;

    float out = Positional_PID(&bldc_yaw_speed_wheel, SetValue, (float)ActualValue, 5);
    return out;
}



//限幅
float limit(float val, float lower, float upper) {
    return val < lower ? lower : (val > upper ? upper : val);
}


